/***************************************************************************\

file      pRTOS.cpp
version   2.0
author    Marten Wensink / Wouter van Ooijen
date      01-04-2010

This file contains the implementation of the RTOS package for the ARM board

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!!                                                                        !!
!! None of the concurrency mechanisms is really blocking.                 !!
!! A task is only blocked after its update-method has run to completion.  !!
!!                                                                        !!
!! Take care that none of the concurrency mechanisms is called directly   !!
!! from an interrupt handler.                                             !!
!!                                                                        !!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

\***************************************************************************/

#include "pRTOS.h"
//
using namespace std;
//
// interrupt interval in us; 100 is still OK, 1 is not
#define TICK_TIME 125

/***************************************************************************\

cTask

\***************************************************************************/

#define nonperiodic_task_logging (global_logging && 0)
#define periodic_task_logging    (global_logging && 0)
#define debug_task_logging       (global_logging && 0)
//
const int cTask::READY      = 0x00;
const int cTask::SUSPENDED  = 0x01;
const int cTask::BLOCKED    = 0x02;
const int cTask::TERMINATED = 0x04;
//
void cTask::debug (const char * msg) {
   if (debug_task_logging) {
	  trace <<  name << " " << msg;
   }
}

cTask::cTask (const char * tname, int prio) :
   status (READY),
   logging (nonperiodic_task_logging),
   name (tname)
{
   debug ("CREATED");
   pRTOS::registerTask (this, prio);
}

cTask::cTask (const char * tname, int prio, int period) :
   status (READY),
   logging (periodic_task_logging),
   name (tname)
{
   debug ("CREATED");
   pRTOS::registerTask (this, prio, period);
}

void cTask::suspend (void) {
   debug ("suspend");
   status &= ~BLOCKED;
   status |= SUSPENDED;
}

void cTask::resume (void) {
   debug ("resume");
   if (status == SUSPENDED)
      status = READY;
}

void cTask::block (void) {
   debug ("block");
   // Only a running task can block itself
   if (pRTOS::getCurrentTask() == this)
      status = BLOCKED;
   else {
      mkt_fatal (#UNIQUE_ERROR); // task not blocked by itself
   }
}

void cTask::unblock (void) {
   debug ("unblock");
   if (!(status & TERMINATED))
      status &= ~BLOCKED;
}

void cTask::terminate (void) {
   status |= TERMINATED;
}

bool cTask::isSuspended (void) {
   return (status & SUSPENDED) == SUSPENDED;
}

bool cTask::isBlocked (void) {
   return (status & BLOCKED) == BLOCKED;
}

bool cTask::isReady (void) {
   return status == READY;
}

const char * cTask::getName (void) {
   return name;
}

void cTask :: statistics_print (std::ostream & stream) {
   stream
      << setw (20) << left  << name
      << setw (12) << right << runtime_max
      << setw (12) << right << activations
      << endl;
}


/***************************************************************************\

cEventFlag

\***************************************************************************/

void cEventFlag::set (void) {
   if (client != 0) {
      client->unblock();
      client = 0;
   }
   else                 
      flag = true;                      
}

void cEventFlag::wait (void) {
   if (flag)       
      flag = false;
      
   else {
      client = pRTOS::getCurrentTask();
      client->block();
   }
}


/***************************************************************************\

cEventFlagGroup

\***************************************************************************/

void cEventFlagGroup::set (int flag) {
   flags |= (1 << flag);
   if (client != 0) {
      client->unblock();
      client = 0;
   }
}

void cEventFlagGroup::wait (void) {
   if (flags == 0) {
      client = pRTOS::getCurrentTask();
      client->block();
   }
}

int cEventFlagGroup::getFlagNo (void) {
   // To be fair, do a cyclic search to determine which flag is set:
   for (int i = 0 ; i < groupSize; i++) {
      if (++flagNo == groupSize)
         flagNo = 0;
      if ((flags & (1 << flagNo)) != 0)
         break;
   }
   // Clear and return the flag set:
   flags &= ~(1 << flagNo);
   return flagNo;
}


/***************************************************************************\

cMutex

\***************************************************************************/

// public operations
void cMutex::wait (void) {
   if (owner == 0)
      owner = pRTOS::getCurrentTask();
   else {
      if (++tail >= MAX_CLIENTS)
         tail = 0;
      queue[tail] = pRTOS::getCurrentTask();
      queue[tail]->block();
      qSize += 1;
   }
}

void cMutex::signal (void) {
   // Only the owner is allowed to signal the mutex
   if (owner == pRTOS::getCurrentTask()) {
      if (qSize > 0) {
         owner = queue[head];
         queue[head]->unblock();
         if (++head >= MAX_CLIENTS)
            head = 0;
         qSize -= 1;
      }
      else
         owner = 0;
   }
   else {
      mkt_fatal (#UNIQUE_ERROR); // mutex blocked not by owner
   }
}


/***************************************************************************\

cTimer

\***************************************************************************/

cTimer::cTimer (void) : nrTicks (0) {
   pRTOS::registerTimer (this);
}


/***************************************************************************\

pRTOS

\***************************************************************************/

// Reference to the task currently executed:
cTask * pRTOS::curTask = 0;

// timer list head
cTimer * pRTOS::timerList = 0;

// periodic task list
cTask * pRTOS::periodicTaskList = 0;

// list of all tasks, highest prority first
cTask * pRTOS::taskList = 0;

// amount of us elapsed since last run of beat
volatile long long int pRTOS::runTime = 0;
volatile int pRTOS::elapsedTime = 0;

// flag set to clear statistics
volatile bool pRTOS::clearing = false;

// interrupt service routine: update times, clear interrupt
void timerISR (void) {
   pRTOS::elapsedTime += TICK_TIME;
   pRTOS::runTime += TICK_TIME;
   T0IR |= 0x01;
}

void pRTOS::statistics_print (ostream & stream) {
   stream
      << setw (20) << left  << "name"
      << setw (12) << right << "runtime_max"
      << setw (12) << right << "activations"
      << endl;

   for (
      cTask * task = taskList;
      task != 0;
      task = task->nextTask
   )
   {  task->statistics_print (stream);
   }
}

void pRTOS::beat (void) {

   // get the elapse time since last beat, and reset it to 0
   mkt_disable_interrupts();
   int elapsed = elapsedTime;
   elapsedTime = 0;
   mkt_enable_interrupts();

   if (elapsed > 0) {
      if (0) {
         trace <<  "elapsed=" << elapsed;
      }
      // update the waiting time of the periodic tasks
      for (
         cTask * task = periodicTaskList;
		   task != 0;
		   task = task->nextPeriodicTask
      )
      {  if (!task->isSuspended()) {
            task->waittime -= elapsed;
         }
         if (0) {
            trace
			      << "periodic task " <<  task->name
			      << " wait=" << task->waittime
			      << " b=" << task->isBlocked();
         }

         if (task->isBlocked()) {
            if (task->waittime <= 0) {
               task->waittime += task->period;
               task->unblock();
               if (0) {
                  trace
				        << "unblock " << task->name
				        <<" new wait=" << task->waittime;
               }
            }
         }
      }

      // service the timer queue
      for (
         cTimer * t = timerList;
         t != 0;
         t = t->nextTimer
      )
      {  t->tick (elapsed);
      }
   }

   // find the highest-priority ready task and run it, then return
   // curTask is used by getCurrentTask
   for (
      curTask = taskList;
      curTask != 0;
      curTask = curTask->nextTask
   )
   {  if (curTask->isReady()) {
         int start = elapsedTime;
         curTask->update();
         int end = elapsedTime;
         int runtime = end - start;
         if (runtime > curTask->runtime_max) {
	        curTask->runtime_max = runtime;
         }
         curTask->activations++;
         if (clearing) {
            do_statistics_clear();
            clearing = false;
         }
         return;
      }
   }
}

void pRTOS::run (void) {

   if (global_logging) {
      trace << "initialize tasks";
   }
   for (
      curTask = taskList;
      curTask != 0;
      curTask = curTask->nextTask
   )
   {  curTask->init();
   }

   if (global_logging) {
      trace << "start the timer isr";
   }
   if (1) {
      mkt_disable_interrupts();
      mkt_vic_int_init (4, 0, timerISR);
      mkt_timer_periodic_int_init (T0, TICK_TIME);
      mkt_enable_interrupts();
   }

   statistics_clear();

   if (global_logging) {
      trace << "scheduler starts";
   }
   int n = 0;
   for (; ;) {
      if (global_logging) {
         if ( ++n > 10000 ) {
      	  std::cout << '.';
      	  n = 0;
         }
      }
      beat();
   }
}

// register a timer
void pRTOS::registerTimer (cTimer * t) {

   // add the timer to the timer list
   t->nextTimer = timerList;
   timerList = t;
}

// register a normal task
void pRTOS::registerTask (cTask * task, int prio) {

   if (global_logging) {
     trace << "register " << task->name;
   }

   task->priority = prio;
   task->period = 0;

   if (taskList == 0) {
      // this is the first task, place it at the head of teh queue
      taskList = task;
 	   task->nextTask = 0;
   }
   else
   {  // walk the task queue untill the next task either
      // - does not exist, or
      // has a lower priority than the new task
      cTask * t = taskList;
      while( ( t->nextTask != 0 ) && ( t->nextTask->priority < prio )) {
         t = t->nextTask;
      }
      // now insert the new task after the current task
      task->nextTask = t->nextTask;
      t->nextTask = task;
   }
}

// register a periodic task
void pRTOS::registerTask (cTask * task, int prio, unsigned period) {

   // register the periodic task also as 'normal' task
   registerTask (task, prio);

   // record the period
   task->period = period;
   task->waittime = task->period;

   // add the periodic task to the period task list
   task->nextPeriodicTask = periodicTaskList;
   periodicTaskList = task;
}

void pRTOS::do_statistics_clear (void) {
   for (
      cTask * task = taskList;
      task != 0;
      task = task->nextTask
   )
   {  task->statistics_clear();
   }
}

